import os
import sys
import time
import math
import select
import tty
import termios

from onboardDevSDK.drone_api import *

###################################################################
# 连接飞控，其他连接方式请参考demo.py
# 这里连接gazebo仿真
dev_addr = "udp://:14550@127.0.0.1:18570"
params = {'sys_id': 1,
          'cmp_id': 200,
          'tgt_sys_id': 1,
          'tgt_cmp_id': 1,
          'fcu_url': dev_addr
          }

drone = DroneAPI(params)


def offboard():
    drone.arm()
    time.sleep(1)
    drone.takeoff()
    time.sleep(5)
    drone.offboard_enable()
    drone.offboard_enable()


def test0():
    print('INFO:test0, print drone status')
    for i in range(20):
        print([a*180/3.14 for a in drone.fcu.attitude_ENU])
        print([a*180/3.14 for a in drone.fcu.attitude_NED])

        time.sleep(0.5)


def test1():
    print('INFO:test1, turn to yaw')
    offboard()
    while True:
        delta = drone.rotateto_yaw(90 * math.pi / 180)
        # delta = drone.turnto_yaw(90 * math.pi / 180)
        print(delta)
        if abs(delta) < 1 * math.pi / 180:
            break
        time.sleep(0.05)


def test2():
    print("INFO:test2, offboard control, fly to a relative height")
    offboard()
    while True:
        dis = drone.moveto_relAlt(10)
        print(dis)
        if abs(dis) < 0.2:
            break
        time.sleep(0.05)


def test3():
    print('INFO:test3, offboard control, fly a rect')
    gps = [(47.3977597, 8.5459459, 15), (47.3979504, 8.5459459, 15),
           (47.3979575, 8.5453954, 10), (47.3976255, 8.5454137, 10)]
    offboard()
    for gp in gps:
        while True:
            dis, alt = drone.moveto_globalPosition_relAlt_yaw(
                gp, heading_mode=1, heading=0.1)
            print("INFO: dis=", dis, alt)
            if dis < 3 and abs(alt) < 1:
                print("INFO: reached wp")
                break
            time.sleep(0.05)
        print('reached', gp)


def test4():
    print('INFO:test4, local position control')
    lps = [(20, 0, -10), (20, 15, -10), (-20, 15, -10), (-20, -15, -10)]

    offboard()
    for lp in lps:
        while True:
            dis, delta = drone.goto_localPosition_pos_yaw(lp)
            print(dis, drone.fcu.local_position)
            if dis < 2:
                break
            time.sleep(0.05)


def test5():
    print('INFO:test5, reset global control')
    gps = [(47.3977597, 8.5459459, 10), (47.3979504, 8.5459459, 12),
           (47.3979575, 8.5453954, 15), (47.3976255, 8.5454137, 10)]
    for gp in gps:
        drone.goto_globalPosition_relAlt_yaw_async(gp)
        time.sleep(10)


if __name__ == '__main__':
    print("INFO: run demo_gazebo.py")
    test0()
    # test1()
    # test2()
    # test3()
    # test4()
    # test5()
